#include "Frustum.h"

namespace TG
{
	Frustum::Frustum(mat4x4 m)
	{
		planes[FRUSTUM_LEFT] = Plane(m[3][0] + m[0][0], m[3][1] + m[0][1], m[3][2] + m[0][2], m[3][3] + m[0][3]);
		planes[FRUSTUM_RIGHT] = Plane(m[3][0] - m[0][0], m[3][1] - m[0][1], m[3][2] - m[0][2], m[3][3] - m[0][3]);

		planes[FRUSTUM_TOP] = Plane(m[3][0] - m[1][0], m[3][1] - m[1][1], m[3][2] - m[1][2], m[3][3] - m[1][3]);
		planes[FRUSTUM_BOTTOM] = Plane(m[3][0] + m[1][0], m[3][1] + m[1][1], m[3][2] + m[1][2], m[3][3] + m[1][3]);

		planes[FRUSTUM_FAR] = Plane(m[3][0] - m[2][0], m[3][1] - m[2][1], m[3][2] - m[2][2], m[3][3] - m[2][3]);
		planes[FRUSTUM_NEAR] = Plane(m[3][0] + m[2][0], m[3][1] + m[2][1], m[3][2] + m[2][2], m[3][3] + m[2][3]);
	}

	bool Frustum::pointInFrustum(const vec3 &v) const
	{
		for (size_t i = FRUSTUM_LEFT; i <= FRUSTUM_NEAR; i++)
		{
			if (planes[i].distance(v) <= 0)
			{
				return false;
			}
		}
		return true;
	}

	bool Frustum::pointInFrustum(const vec4 &v) const
	{
		return pointInFrustum(vec3(v.x, v.y, v.z));
	}

	bool Frustum::cubeInFrustum(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax) const
	{
		for (size_t i = FRUSTUM_LEFT; i <= FRUSTUM_NEAR; i++)
		{
			int c = 0;
			c += planes[i].distance(vec3(xMin, yMin, zMin)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMax, yMin, zMin)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMax, yMax, zMin)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMin, yMax, zMin)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMin, yMin, zMax)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMax, yMin, zMax)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMax, yMax, zMax)) > 0 ? 1 : -1;
			c += planes[i].distance(vec3(xMin, yMax, zMax)) > 0 ? 1 : -1;

			if (c == 8 || c == -8)
			{
				return false;
			}
		}
		return true;
	}

	bool Frustum::cubeInFrustum(const AABB &aabb) const
	{
		return cubeInFrustum(aabb._min.x, aabb._max.x, aabb._min.y, aabb._max.y, aabb._min.z, aabb._max.z);
	}

	bool Frustum::cubeCrossFrustum(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax) const
	{
		return true;
	}
}